/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/MethodSrc/particle.cpp
 * @Description  : 粒子类实现，包含粒子的初始化、更新、配置及状态管理等功能。 
 *                 用于粒子群优化算法中的粒子表示，支持动态内存管理和适应度函数的调用。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:42:30
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "particle.h"
using namespace std;
 
// Static member initialization
int Particle::number_of_variables_ = 1;
const double* Particle::lower_bounds_ = nullptr;
const double* Particle::upper_bounds_ = nullptr;
const double* Particle::max_velocity_ = nullptr;
double Particle::c1_ = 2; 
double Particle::c2_ = 2;
double Particle::factor_ = 1;  // 0.729
const double* Particle::weight_ = nullptr;
void (*Particle::function_ptr_)(Particle*) = nullptr;

/**
 * @brief 粒子类的构造函数
 *
 * 初始化粒子对象，包括粒子的位置、速度、最优位置等属性。
 *
 * @param num_variables 粒子变量的数量
 * @param lower_bounds 粒子变量下界的指针
 * @param upper_bounds 粒子变量上界的指针
 * @param vmax 粒子速度的最大值数组指针
 * @param c1 认知学习因子
 * @param c2 社会学习因子
 * @param weight 粒子的权重数组指针
 * @param function_ptr 指向适应度函数的指针
 */
Particle::Particle(int num_variables, const double* lower_bounds, const double* upper_bounds,
                   const double* vmax, double c1, double c2, const double* weight,
                   void (*function_ptr)(Particle*)) 
                   : error_(PARTICLE_NO_ERROR), feasible_(true) 
{
    lower_bounds_ = lower_bounds;
    upper_bounds_ = upper_bounds;
    number_of_variables_ = num_variables;
    if (number_of_variables_ <= 0) {
        error_ = PARTICLE_VARIABLE_INVALID;
        position_ = nullptr;
        velocity_ = nullptr;
        best_position_ = nullptr;
    } else {
        c1_ = c1;
        c2_ = c2;
        weight_ = weight;
        function_ptr_ = function_ptr;
        max_velocity_ = vmax;
        error_ = PARTICLE_NO_ERROR;
        position_ = new double[number_of_variables_];
        velocity_ = new double[number_of_variables_];
        best_position_ = new double[number_of_variables_];
        InitializeParticle();
    }
}

/**
 * @brief 析构函数
 *
 * 释放Particle对象中动态分配的内存
 */
Particle::~Particle() 
{
    delete[] position_;
    delete[] velocity_;
    delete[] best_position_;
}

/**
 * @brief 初始化粒子
 *
 * 初始化粒子的位置和速度，并调用用户定义的目标函数来更新粒子的适应度值
 *
 * 如果粒子存在错误，则不进行初始化
 *
 * @return void
 */
void Particle::InitializeParticle() 
{
    if (error_ != PARTICLE_NO_ERROR) return;

    for (int i = 0; i < number_of_variables_; ++i) {
        double random1 = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX) + 1);
        double random2 = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX) + 1);
        position_[i] = lower_bounds_[i] + (upper_bounds_[i] - lower_bounds_[i]) * random1;
        velocity_[i] = -max_velocity_[i] + 2 * max_velocity_[i] * random2;
    }
    (*function_ptr_)(this);
    best_value_ = value_;
    best_feasible_ = feasible_;
    for (int i = 0; i < number_of_variables_; ++i) {
        best_position_[i] = position_[i];
    }
}

/**
 * @brief 配置粒子状态
 *
 * 该函数用于配置粒子的位置和速度，并调用相应的函数指针来更新粒子的状态。
 *
 * @param pos 指向粒子位置的指针数组
 * @param vel 指向粒子速度的指针数组
 */
void Particle::ConfigureParticle(const double* pos, const double* vel) 
{
    if (error_ != PARTICLE_NO_ERROR) return;

    for (int i = 0; i < number_of_variables_; ++i) {
        position_[i] = pos[i];
        velocity_[i] = vel[i];
    }
    (*function_ptr_)(this);
    best_value_ = value_;
    best_feasible_ = feasible_;
    for (int i = 0; i < number_of_variables_; ++i) {
        best_position_[i] = position_[i];
    }
}

/**
 * @brief 更新粒子位置和速度
 *
 * 更新粒子的速度和位置，并检查其是否超出边界。
 * 如果粒子的状态正常（即没有错误），则根据公式更新粒子的速度，
 * 然后根据速度更新位置，并确保位置在允许的范围内。
 * 最后，调用适应度函数更新粒子的值，并更新粒子的最优位置。
 *
 * @param global_best_position 全局最优位置数组
 */
void Particle::UpdateParticle(const double* global_best_position) 
{
    if (error_ != PARTICLE_NO_ERROR) return;

    for (int i = 0; i < number_of_variables_; ++i) {
        double random1 = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX) + 1);
        double random2 = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX) + 1);
        velocity_[i] = *weight_ * velocity_[i] + c1_ * random1 * (best_position_[i] - position_[i]) +
                        c2_ * random2 * (global_best_position[i] - position_[i]);
        if (fabs(velocity_[i]) > max_velocity_[i]) {
            velocity_[i] = max_velocity_[i] * velocity_[i] / fabs(velocity_[i]);
        }
        position_[i] = position_[i] + factor_ * velocity_[i];
        if (position_[i] >= upper_bounds_[i]) position_[i] = upper_bounds_[i];
        if (position_[i] <= lower_bounds_[i]) position_[i] = lower_bounds_[i];
    }
    (*function_ptr_)(this);
    value_ += constraints_;
    UpdateParticleBest();
}

/**
 * @brief 获取粒子的最佳位置
 *
 * 返回粒子的最佳位置指针。
 *
 * @return 返回粒子的最佳位置的指针。
 */
const double* Particle::GetBestPosition() const 
{
    return best_position_;
}

/**
 * @brief 获取粒子最佳值
 *
 * 返回粒子的最佳值。
 *
 * @return double 粒子的最佳值
 */
double Particle::GetBestValue() const 
{
    return best_value_;
}

/**
 * @brief 获取当前粒子是否为最佳可行解
 *
 * 该函数用于判断当前粒子是否为最佳可行解。
 *
 * @return 如果当前粒子为最佳可行解，则返回true；否则返回false。
 */
bool Particle::GetBestFeasible() const 
{
    return best_feasible_;
}

/**
 * @brief 获取粒子的值
 *
 * 返回粒子的值。
 *
 * @return 粒子的值
 */
double Particle::GetValue() const 
{
    return value_;
}

/**
 * @brief 设置粒子的值
 *
 * 该函数用于设置粒子的值。
 *
 * @param value 要设置的值
 */
void Particle::SetValue(double value) 
{
    value_ = value;
}

/**
 * @brief 设置粒子的约束条件
 *
 * 设置粒子的约束条件，用于后续计算或模拟中。
 *
 * @param constraints 约束条件的值
 */
void Particle::SetConstraints(double constraints) 
{
    constraints_ = constraints;
}

/**
 * @brief 判断粒子是否可行
 *
 * 检查粒子是否满足特定条件，判断其是否可行。
 *
 * @return 如果粒子可行，则返回true；否则返回false。
 */
bool Particle::GetFeasible() const 
{
    return feasible_;
}

/**
 * @brief 获取粒子对象的错误码
 *
 * 返回粒子对象的错误码。
 *
 * @return 返回错误码
 */
int Particle::GetError() const 
{
    return error_;
}

/**
 * @brief 设置粒子是否可行
 *
 * 设置粒子的可行性状态。
 *
 * @param feasible 粒子的可行性状态
 *     - true: 粒子可行
 *     - false: 粒子不可行
 */
void Particle::SetFeasible(bool feasible) 
{
    feasible_ = feasible;
}

/**
 * @brief 设置粒子的错误信息
 *
 * 该函数用于设置粒子的错误信息。
 *
 * @param err 错误代码
 */
void Particle::SetError(int err) 
{
    error_ = err;
}

/**
 * @brief 打印粒子信息
 *
 * 打印粒子的位置、值、约束条件和可行性信息。
 */
void Particle::PrintParticle() const 
{
    std::cout << "Position = ";
    for (int i = 0; i < number_of_variables_; ++i) {
        std::cout << position_[i] << "   ";
    }
    std::cout << std::endl;
    std::cout << "Value = " << value_ << std::endl;
    std::cout << "Constraints = " << constraints_ << std::endl;
    std::cout << "Feasible? " << (feasible_ ? "true" : "false") << std::endl;
}

/**
 * @brief 打印粒子的最优位置和最优值
 *
 * 打印粒子的最优位置、最优值和可行性。
 */
void Particle::PrintParticleBest() const 
{
    std::cout << "Position = ";
    for (int i = 0; i < number_of_variables_; ++i) {
        std::cout << best_position_[i] << "   ";
    }
    std::cout << std::endl;
    std::cout << "Value = " << best_value_ << std::endl;
    std::cout << "Feasible? " << (best_feasible_ ? "true" : "false") << std::endl;
}

/**
 * @brief 获取变量的数量
 *
 * 返回当前粒子中变量的数量。
 *
 * @return 返回变量的数量
 */
int Particle::GetNumberOfVariables() 
{
    return number_of_variables_;
}

/**
 * @brief 获取粒子的位置
 *
 * 返回粒子的位置指针。
 *
 * @return 返回粒子的位置指针
 */
const double* Particle::GetPosition() const 
{
    return position_;
}

/**
 * @brief 更新粒子的最优解
 *
 * 该函数用于更新粒子的最优解。
 *
 * 如果当前粒子是可行的或者当前最优解不可行，
 * 并且当前粒子的适应度值小于等于最优解的适应度值，
 * 则更新最优解为当前粒子。
 *
 */
void Particle::UpdateParticleBest() 
{
    if (feasible_ || !best_feasible_) {
        if (feasible_ != best_feasible_ || value_ <= best_value_) {
            best_value_ = value_;
            best_feasible_ = feasible_;
            for (int i = 0; i < number_of_variables_; ++i) {
                best_position_[i] = position_[i];
            }
        }
    }
}